import EDU.gatech.cc.is.util.Vec2;


public class Vec2D {

	Vec2 v = new Vec2();
	
	private Vec2D(double x, double y){
		v = new Vec2(x, y);
	}
	public Vec2D(Vec2 vec){ v = vec; } 
	public Vec2D(Vec2D vec){ v = clone().Vec2(); } 
	static public Vec2D cortezian(double x, double y){ return new Vec2D(x, y); }
	static public Vec2D polar(double t, double r){ Vec2D v = new Vec2D(1,0); v.t(t); v.r(r); return v; }
	static public Vec2D fromVec2(Vec2 v){ return new Vec2D(v); }
	
	//public String toString(){ return v.toString(); }
	public String toString(){
		String s = "";
		s+="C("+String.format("%.2f", x())+","+String.format("%.2f", y())+") ";
		s+="P("+String.format("%.1f", r2d(h()))+"d,"+String.format("%.2f", r())+")";
		return s;
	}
	
	public void setVec2(Vec2 vv){ v=vv; }
	public void set(Vec2D vv){ v=vv.v; }
	public Vec2 Vec2(){ Vec2 vv = new Vec2(v.x, v.y); return vv; }
	public void Vec2(Vec2 vv){ Vec2D w = new Vec2D(v.x, v.y); v = w.v; }
	public void x(double x){ v.setx(x); }
	public void y(double y){ v.sety(y); }
	public void t(double t){ v.sett(t); }
	public void r(double r){ v.setr(Math.abs(r)); if(r<0) v.rotate(Math.PI); }
	public void xy(double x, double y){ v = new Vec2(x,y); }
	public void tr(double t, double r){ Vec2D vv = polar(t, r); v=vv.v; }
	public Vec2D clone(){ return new Vec2D(v.x, v.y); }
	public Vec2D rotated(double t){ Vec2D vv = clone(); vv.v.rotate(t); return vv; }
	public Vec2D sub(Vec2D vv){ Vec2D vvv = clone(); vvv.v.sub(vv.v); return vvv; }
	public Vec2D add(Vec2D vv){ Vec2D vvv = clone(); vvv.v.add(vv.v); return vvv; }
	public Vec2D scale(double x, double y){ Vec2D vvv = clone(); vvv.v.setx(vvv.v.x*x);vvv.v.sety(vvv.v.y*y); return vvv; }
	public Vec2D scale(double f){ return scale(f, f); }
	public Vec2D scaleR(double f){ return Vec2D.polar(t(), r()*f); }
	public double x(){ return v.x; }
	public double y(){ return v.y; }
	public double t(){ return v.t; }
	public double r(){ return v.r; }
	public double ang(){ return h(); }
	public double len(){ return r(); }
	
	public Vec2D changedX(double q){ return cortezian(q, v.y); }
	public Vec2D changedY(double q){ return cortezian(v.x, q); }
	public Vec2D changedT(double q){ return polar(q,v.r); }
	public Vec2D changedR(double q){ return polar(v.t,q); }
	
	public static double heading(double a){ 
		double t = polar(a, 1).t();
		if(t<=Math.PI) return t;
		return -(2*Math.PI - t);
	}
	public double h(){ return heading(t()); }
	
	public boolean equals(Object o){
		if(o instanceof Vec2D == false) return false;
		Vec2D v = (Vec2D)o;
		return sub(v).r()<0.0001;
	}
	
	public static double r2d(double rad){ return rad*57.2957795;}
	public static double d2r(double deg){ return deg*0.0174532925;}
	
	public Vec2D toLocal(Vec2D v, double h){ return sub(v).rotated(-h); }
	public Vec2D toGlobal(Vec2D v, double h){ return rotated(h).add(v); }
	
	public static double distance(Vec2D v1, Vec2D v2){ return v2.sub(v1).r(); }
	
	public static class Pose{
		public Vec2D loc;
		public double heading;
		public Pose(Vec2D l, double h){ loc=l; heading=h; }
		public Pose add(Pose center){ return new Pose(loc.toGlobal(center.loc, center.heading), heading+center.heading);}
		public Pose sub(Pose center){ return new Pose(loc.toLocal(center.loc, center.heading), heading-center.heading);}
		public String toString(){ return "["+loc.toString()+":"+String.format("%.2f", heading)+"]"; };
	}
}
